#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
import time
import os
from ament_index_python.packages import get_package_share_directory
from .ardSerial import *
from .SerialCommunication import Communication

class DogControlNode(Node):
    def __init__(self, uart_dev, baudrate):
        super().__init__('dog_control')
        self.get_logger().info("Initializing dog control node...")
        
        self.goodPorts = {}
        self.flag = 0
        
        try:
            # 初始化串口
            self.serial_obj = Communication(uart_dev, baudrate, 1)
            if not self.serial_obj.main_engine.is_open:
                raise RuntimeError(f"Failed to open {uart_dev}")
            
            self.goodPorts[self.serial_obj] = uart_dev.split('/')[-1]
            self.get_logger().info(f"Serial port {uart_dev} opened successfully")
            
            # 等待串口稳定
            time.sleep(2)
            
        except Exception as e:
            self.get_logger().error(f"Failed to initialize serial port: {e}")
            raise

        # 创建定时器
        self.timer_send = self.create_timer(2.0, self.timer_send_callback)
        self.timer_receive = self.create_timer(0.1, self.timer_receive_callback)

    def timer_send_callback(self):
        """定时发送控制命令的回调函数"""
        try:
            if self.flag == 0:
                # 控制所有舵机（8-15号）到指定角度
                commands = [
                    "m 8 30\n",   # 肩部舵机
                    "m 9 30\n",   # 肩部舵机
                    "m 10 20\n",  # 腿部舵机
                    "m 11 20\n",  # 腿部舵机
                    "m 12 0\n",  # 脚部舵机
                    "m 13 0\n",  # 脚部舵机
                    "m 14 0\n",  # 脚部舵机
                    "m 15 0\n"   # 脚部舵机
                ]
            else:
                # 控制所有舵机到另一个位置
                commands = [
                    "m 8 -15\n",
                    "m 9 -15\n",
                    "m 10 -20\n",
                    "m 11 -20\n",
                    "m 12 -35\n",
                    "m 13 -35\n",
                    "m 14 50\n",
                    "m 15 50\n"
                ]
                
            self.get_logger().info("Sending multiple servo commands")
            
            # 快速连续发送所有命令
            for cmd in commands:
                self.serial_obj.Send_data(cmd.encode())
                time.sleep(0.04)  # 延迟
            
            # 等待所有命令执行完成
            time.sleep(1.2)
            self.flag = not self.flag
            
        except Exception as e:
            self.get_logger().error(f"Error in timer_send_callback: {str(e)}")
            import traceback
            self.get_logger().error(traceback.format_exc())

    def timer_receive_callback(self):
        try:
            if self.serial_obj.main_engine.in_waiting > 0:
                received_data = self.serial_obj.Read_Line()
                received_string = received_data.decode('ISO-8859-1')
                self.get_logger().info(f"Received: {received_string}")
        except Exception as e:
            self.get_logger().error(f"Serial receive error: {e}")

    def __del__(self):
        try:
            if hasattr(self, 'goodPorts') and len(self.goodPorts) > 0:
                # 发送平衡姿态命令
                try:
                    # 创建平衡姿态的舵机数组
                    balance_array = [0] * 16
                    send(self.goodPorts, ['L', balance_array, 2])
                except Exception as e:
                    self.get_logger().error(f"Failed to send balance command: {e}")
                
                # 关闭所有串口
                closeAllSerial(self.goodPorts)
                self.get_logger().info("Serial ports closed")
        except Exception as e:
            self.get_logger().error(f"Error during cleanup: {e}")

def main(args=None):
    """主函数"""
    try:
        # 初始化ROS2
        rclpy.init(args=args)
        
        # 串口设备名称和波特率
        uart_dev = "/dev/ttyS1"  # 根据实际情况修改
        baudrate = 115200
        
        # 创建节点实例
        node = DogControlNode(uart_dev, baudrate)
        
        try:
            # 运行节点
            rclpy.spin(node)
        except KeyboardInterrupt:
            closeAllSerial(goodPorts)
            node.get_logger().info("Node stopped cleanly")
        except Exception as e:
            node.get_logger().error(f"Error during node execution: {e}")
        finally:
            # 清理节点
            node.destroy_node()
            rclpy.shutdown()
            
    except Exception as e:
        print(f"Failed to start node: {e}")
        if rclpy.ok():
            rclpy.shutdown()

if __name__ == '__main__':
    main()